#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/registration/gicp.h>
#include <jsk_recognition_msgs/BoundingBoxArray.h>
#include <geometry_utils/Transform3.h>
#include <sstream>
#include <fstream>
#include <iostream>
#include <string>
#include "time_counter.hpp"
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <queue>
#include <thread>
#include <mutex>
#include <csignal>
#include <point2pixel_pin.h>
#include <point2pixel_fish.h>
#include "common.h"

using namespace std;

std::string itos(int i) {
    std::stringstream s;
    s << i;
    return s.str();
}

template<typename Point>
void pubpc(ros::Publisher &pub, pcl::PointCloud<Point> &pc, std::string frame = "base_link") {
    sensor_msgs::PointCloud2 msgs;
    pcl::toROSMsg(pc, msgs);
    msgs.header.frame_id = frame;
    pub.publish(msgs);
}


std::shared_ptr<Point2Pixel<pcl::PointXYZI>> p2p_ptr;
ros::Publisher pub_cloud;
ros::Publisher pub_cloud2;
ros::Publisher pub_image;
ros::Publisher pub_box;

int loadData(const char *file, void **data, unsigned int *length)
{
    std::fstream dataFile(file, std::ifstream::in);

    if (!dataFile.is_open())
    {
        std::cout << "Can't open files: " << file << std::endl;
        return -1;
    }

    // get length of file:
    unsigned int len = 0;
    dataFile.seekg(0, dataFile.end);
    len = dataFile.tellg();
    dataFile.seekg(0, dataFile.beg);

    // allocate memory:
    char *buffer = new char[len];
    if (buffer == NULL)
    {
        std::cout << "Can't malloc buffer." << std::endl;
        dataFile.close();
        exit(-1);
    }

    // read data as a block:
    dataFile.read(buffer, len);
    dataFile.close();

    *data = (void *)buffer;
    *length = len;
    return 0;
}

struct PointXYZI
{
    float x;
    float y;
    float z;
    float intensity;
};

void bin2pcl(const std::string& input_file_path,pcl::PointCloud<pcl::PointXYZI>& cloud){
    std::ifstream input_file(input_file_path, std::ios::binary);

    if (!input_file.is_open())
    {
        std::cerr << "Failed to open input file: " << input_file_path << std::endl;
        return;
    }

    input_file.seekg(0, std::ios::end);
    size_t file_size = input_file.tellg();
    input_file.seekg(0, std::ios::beg);

    int num_points = file_size / sizeof(PointXYZI);
    PointXYZI *points = new PointXYZI[num_points];

    input_file.read(reinterpret_cast<char *>(points), file_size);
    input_file.close();

    for (int i = 0; i < num_points; ++i)
    {
        pcl::PointXYZI p;
        p.x = points[i].x;
        p.y = points[i].y;
        p.z = points[i].z;
        p.intensity = points[i].intensity;
        cloud.push_back(p);
    }
}

void readseg(const std::string& path,vector<int>& segs){
    segs.clear();
    std::ifstream file;
    file.open(path);
    std::string line;
    while(getline(file,line)){
        std::stringstream ss;
        ss.str(line);
        vector<int> datas;
        string data;
        while(getline(ss,data,'\t')){
            datas.push_back(atoi(data.data()));
        }
        ss.clear();
        segs.push_back(datas[0]);
    }
}

struct bbox{
    string Class;
    float len;
    float wid;
    float height;
    float x;
    float y;
    float z;
    float yaw;
};

void readbox(const std::string& path,vector<bbox>& boxes){
    boxes.clear();
    std::ifstream file;
    file.open(path);
    std::string line;
    while(getline(file,line)){
        bbox box;
        std::stringstream ss;
        ss.str(line);
        vector<string> datas;
        string data;
        while(getline(ss,data,'\t')){
            datas.push_back(data);
        }
        ss.clear();
        box.Class = datas[0];
        box.len = atof(datas[2].data());
        box.wid = atof(datas[3].data());
        box.height = atof(datas[4].data());
        box.x = atof(datas[5].data());
        box.y = atof(datas[6].data());
        box.z = atof(datas[7].data());
        box.yaw = atof(datas[9].data())/180.0*M_PI;
        boxes.push_back(box);
    }
}

void loop() {
    int i = 0;
    const int jump = 5;
    std::string file_path = "/media/zwh/T7/ZWH/bags/dataset/pandarset_det_seg/";
    std::string savefile_path_cloud = file_path + "yolop/cloud/train/";
    std::string savefile_path_boxes = file_path + "yolop/det_annotations/train/";
    std::string savefile_path_seg = file_path + "yolop/da_seg_annotations/train/";
    std::string file_path_cloud = file_path + "cloudxyzi";
    std::string file_path_boxes = file_path + "label_box";
    std::string file_path_image = file_path + "front_img_flp";
    std::string file_path_seg = file_path + "seg";
    std::vector<std::string> cloudPath;
    std::vector<std::string> boxPath;
    std::vector<std::string> imgPath;
    std::vector<std::string> segPath;
    get_alldata_path(file_path_cloud, cloudPath,"bin");
    get_alldata_path(file_path_boxes, boxPath,"txt");
    get_alldata_path(file_path_image, imgPath,"jpg");
    get_alldata_path(file_path_seg, segPath,"txt");

    std::size_t num = cloudPath.size();
    assert(boxPath.size()==num&& imgPath.size()==num && segPath.size()==num);

    float rate=0.9;//训练数据占比
    std::size_t num_train = rate*num;
    ros::Rate loop_rate(50);
    for(std::size_t i=1009;i<num;i++){
        if(i==num_train){
            savefile_path_cloud.replace(savefile_path_cloud.size()-6,6,"val/");
            savefile_path_boxes.replace(savefile_path_boxes.size()-6,6,"val/");
            savefile_path_seg.replace(savefile_path_seg.size()-6,6,"val/");
        }

        std::cout<<"遍历点云:"<<cloudPath[i]<<std::endl;
        //读数据  点云、图片、seg、box
        pcl::PointCloud<pcl::PointXYZI> cloud;
        cv::Mat image;
        bin2pcl(cloudPath[i],cloud);
        image = cv::imread(imgPath[i]);

        vector<int> segs;
        readseg(segPath[i],segs);
        assert(segs.size()==cloud.points.size());

        vector<bbox> boxes;
        readbox(boxPath[i],boxes);

        // 点云图像投影
        pcl::PointCloud<pcl::PointXYZRGBA> rgbacloud;
        pcl::PointCloud<pcl::PointXYZI> cloud2;
        for (size_t j=0;j< cloud.size();j++) {
            const auto& pt = cloud[j];
            pcl::PointXYZRGBA rgba_pt;
            pcl::PointXYZI xyzi_pt;
            cv::Point2d pt2d = p2p_ptr->loop(pt);
            if (pt.x >0 && pt2d.x > 0 && pt2d.x < 1920 && pt2d.y > 0 && pt2d.y < 1080) {
                rgba_pt.x = pt.x;
                rgba_pt.y = pt.y;
                rgba_pt.z = pt.z;
                rgba_pt.b = image.at<cv::Vec3b>(pt2d.y, pt2d.x)[0];
                rgba_pt.g = image.at<cv::Vec3b>(pt2d.y, pt2d.x)[1];
                rgba_pt.r = image.at<cv::Vec3b>(pt2d.y, pt2d.x)[2];
                rgba_pt.a = pt.intensity;//我把强度存在a中 0-255
                rgbacloud.push_back(rgba_pt);//彩色点云
                xyzi_pt.x = pt.x;
                xyzi_pt.y = pt.y;
                xyzi_pt.z = pt.z;
                xyzi_pt.intensity = segs[j];
                cloud2.points.push_back(xyzi_pt);//分割点云
            }
        }
        // 过滤box
        vector<bbox> boxes2;
        boxes2.swap(boxes);
        for(const auto& box:boxes2){
            pcl::PointXYZI pt;
            pt.x = box.x;
            pt.y = box.y;
            pt.z = box.z;
            cv::Point2d pt2d = p2p_ptr->loop(pt);
            if (pt.x >0 && pt2d.x > 0 && pt2d.x < 1920 && pt2d.y > 0 && pt2d.y < 1080) {
                boxes.push_back(box);
            }
        }

        string file_name = cloudPath[i].substr(cloudPath[i].size()-14,10);
        ofstream cloud_file,segfile,detfile;
        cloud_file.open(savefile_path_cloud+file_name+".bin",ios::out | ios::binary);//对bin文件注意存储格式，读取和存储应该一致
        segfile.open(savefile_path_seg+file_name+".txt");
        detfile.open(savefile_path_boxes+file_name+".txt");
        for(size_t j=0;j< rgbacloud.size();j++){
            pcl::PointXYZRGBA& rgbpt= rgbacloud.points[j];
            pcl::PointXYZI& pt= cloud2.points[j];
//            cloud_file.write((char*)&rgbpt.x, sizeof(rgbpt.x));
            cloud_file.write((char*)&rgbpt.x, sizeof(rgbpt.x));
            cloud_file.write((char*)&rgbpt.y, sizeof(rgbpt.y));
            cloud_file.write((char*)&rgbpt.z, sizeof(rgbpt.z));
            float temp = float(rgbpt.a);
            cloud_file.write((char*)&temp, sizeof(temp));
            temp = float(rgbpt.r);
            cloud_file.write((char*)&temp, sizeof(temp));
            temp = float(rgbpt.g);
            cloud_file.write((char*)&temp, sizeof(temp));
            temp = float(rgbpt.b);
            cloud_file.write((char*)&temp, sizeof(temp));
//            cloud_file<< float(rgbpt.x)<<float(rgbpt.y)<<float(rgbpt.z)<<float(rgbpt.a)<<float(rgbpt.r)<<float(rgbpt.g)<<float(rgbpt.b);
            segfile<< int(pt.intensity)<<"\n";
        }
        for(auto& box:boxes){
            detfile<< box.Class<<"\t"<<0<<"\t"<<box.len<<"\t"<<box.wid<<"\t"<<box.height<<"\t"<<box.x<<"\t"<<box.y<<"\t"<<box.z<<"\t"<<0<<"\t"<<box.yaw<<"\n";
        }
        cloud_file.close();
        segfile.close();
        detfile.close();

        sensor_msgs::PointCloud2 msgs;
        pcl::toROSMsg(rgbacloud, msgs);
        msgs.header.frame_id = "base_link";
        pub_cloud.publish(msgs);
        pcl::toROSMsg(cloud2, msgs);
        msgs.header.frame_id = "base_link";
        pub_cloud2.publish(msgs);
        sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
        pub_image.publish(*msg);

        jsk_recognition_msgs::BoundingBoxArray box_array;
        box_array.header.frame_id = "base_link";
        box_array.header.stamp = ros::Time::now();
        for(const auto& boxinfo:boxes){
            jsk_recognition_msgs::BoundingBox box;
            box.dimensions.x = boxinfo.len;
            box.dimensions.y = boxinfo.wid;
            box.dimensions.z = boxinfo.height;
            box.pose.position.x = boxinfo.x;
            box.pose.position.y = boxinfo.y;
            box.pose.position.z = boxinfo.z;
            float yaw = boxinfo.yaw;
            box.pose.orientation.w = cos(yaw / 2.0);
            box.pose.orientation.z = sin(yaw / 2.0);

            box.header.frame_id = "base_link";
            box.header.stamp = ros::Time::now();
            box_array.boxes.push_back(box);
        }
        pub_box.publish(box_array);

        printf("ddd\n");
        loop_rate.sleep();
    }
}


int main(int argc, char **argv) {

    ros::init(argc, argv, "bag2pcd");
    ros::NodeHandle nh;
    pub_cloud = nh.advertise<sensor_msgs::PointCloud2>("/rgb_cloud", 1);
    pub_cloud2 = nh.advertise<sensor_msgs::PointCloud2>("/seg_cloud", 1);
    pub_image = nh.advertise<sensor_msgs::Image>("/raw_img", 1);
    pub_box = nh.advertise<jsk_recognition_msgs::BoundingBoxArray>("/bounding_boxes", 1);

    p2p_ptr.reset(new Point2PixelPin());

    std::string sensor_path = "/home/zwh/work_space/18xx/lidar_tool_ws/src/lidar_tool/p2p/pandarset.yaml";
    cv::FileStorage fs(sensor_path, cv::FileStorage::READ);
    std::cout << "参数文件来自:" << sensor_path << std::endl;
    std::string camera_id = "P1";

    bool isfish;
    std::string pos;
    cv::Mat base2camera;
    cv::Mat cameraMatrix;
    cv::Mat distCoeffs;
    isfish = camera_id[0] == 'F';
    std::string strId;
    for (size_t i = 1; i < camera_id.size(); i++) {
        strId += camera_id[i];
    }
    int id = std::stoi(strId);
    fs["Camera"][camera_id]["pos"] >> pos;
    fs["Camera"][camera_id]["xyzrpy"] >> base2camera;
    fs["Camera"][camera_id]["cameraMatrix"] >> cameraMatrix;
    fs["Camera"][camera_id]["distCoeffs"] >> distCoeffs;
    std::cout << "相机id:" << camera_id << "  isfish:" << isfish << "\nbase2camera\n"
              << base2camera
              << "\ncameraMatrix\n"
              << cameraMatrix
              << "\ndistCoeffs\n"
              << distCoeffs << std::endl;
    assert((isfish && distCoeffs.rows == 4) || (!isfish && distCoeffs.rows == 5)); //鱼眼畸变参数4个，针孔畸变参数5个

    Eigen::Affine3f transform_A; // base_link to 相机外参矩阵
    transform_A = Eigen::Affine3f::Identity();
    transform_A.translation() << base2camera.at<double>(0, 0), base2camera.at<double>(1, 0), base2camera.at<double>(2,
                                                                                                                    0);
    transform_A.rotate(Eigen::AngleAxisf(base2camera.at<double>(5, 0), Eigen::Vector3f::UnitZ()));
    transform_A.rotate(Eigen::AngleAxisf(base2camera.at<double>(4, 0), Eigen::Vector3f::UnitY()));
    transform_A.rotate(Eigen::AngleAxisf(base2camera.at<double>(3, 0), Eigen::Vector3f::UnitX()));

    p2p_ptr->init(transform_A, cameraMatrix, distCoeffs, pos, id);

    loop();

    return 0;
}
